/*!
  @file detector_server.cc
  @brief Brief description
  @author Kyohei Otsu <kyohei@kth.se>
  @date 2013
*/

#include <ros/ros.h>

#include "mmo_vo/DetectorInquiry.h"

using namespace mmo_vo;

namespace vo
{

//! class description 
/*! This is a detailed description of the class */
class DetectorServer {
 private:
  //! ROS node handler
  ros::NodeHandle nh_;
  //! ROS node handler in the private namespace
  ros::NodeHandle private_nh_;
  //! 
  ros::ServiceServer server;

  //! current detector
  int detector_id_;

 protected:

 public:
  //! Constructor
  DetectorServer(ros::NodeHandle &nh) : nh_(nh), private_nh_("~")
  {
    // initializing pub/sub, server
    server = nh_.advertiseService(nh_.resolveName("detector_in"), &DetectorServer::service_cb, this);

    // initializing optional parameters
    // e.g.) private_nh_.param<TYPE>("PARAM_NAME", PARAM_VAR, DEFAULT_VALUE);
    private_nh_.param<int>("default_detector", detector_id_, DetectorInquiry::Response::FEATURE_HARRIS);
  }

  //! Destructor
  ~DetectorServer()
  {
  }

  //! callback
  bool service_cb(DetectorInquiry::Request &request, DetectorInquiry::Response &response)
  {
    if(request.id == DetectorInquiry::Request::INQUIRY)
    {
      ROS_INFO_STREAM("Receive detector inquiry. Current detector is " << detector_id_);
      response.id = detector_id_;
      return true;
    }
    else
    {
      detector_id_ = request.id;
      ROS_INFO_STREAM("Receive detector change. Current detector is " << detector_id_);
      response.id = detector_id_;
      return true;
    }
  }
};

} // end of namespace

int main(int ac, char **av)
{
  ros::init(ac, av, "DetectorServer");
  ros::NodeHandle nh;
  vo::DetectorServer handler(nh);
  ros::spin();
  return 0;
}

